/*
 * Copyright (c) 2011 Eugenio Realini, Mirko Reguzzoni, Cryms sagl - Switzerland. All Rights Reserved.
 *
 * This file is part of goGPS Project (goGPS).
 *
 * goGPS is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as
 * published by the Free Software Foundation, either version 3
 * of the License, or (at your option) any later version.
 *
 * goGPS is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with goGPS.  If not, see <http://www.gnu.org/licenses/>.
 *
 */
package com.galfins.gogpsextracts.Glonass;

import com.galfins.gogpsextracts.Constants;
import com.galfins.gogpsextracts.Glonass.EphGlonass;
import com.galfins.gogpsextracts.Observations;
import com.galfins.gogpsextracts.SatellitePosition;
import com.galfins.gogpsextracts.Time;

import java.util.Arrays;

import org.ejml.simple.SimpleMatrix;

/**
 * <p>
 *
 * </p>
 *
 * @author Eugenio Realini, Lorenzo Patocchi (code architecture)
 */

public abstract class EphemerisSystemGlonass {

    /**
     * @param time      (GPS time in seconds)
     * @param satID
     * @param range
     * @param approxPos
     */

    static double dot(double a[], double b[], int n) {
        double c = 0.0;

        while (--n >= 0) c += a[n] * b[n];
        return c;
    }

    static double SQR(double x) {
        return x * x;
    }

    static void deq(double x[], double xdot[], double acc[]) {
        double OMGE_GLO = 7.292115E-5;
        double J2_GLO = 1.0826257E-3;
        double MU_GLO = 3.9860044E14;
        double RE_GLO = 6378136.0;
        double a, b, c, r2 = dot(x, x, 3), r3 = r2 * Math.sqrt(r2), omg2 = SQR(OMGE_GLO);

        if (r2 <= 0.0) {
            xdot[0] = xdot[1] = xdot[2] = xdot[3] = xdot[4] = xdot[5] = 0.0;
            return;
        }
        /* ref [2] A.3.1.2 with bug fix for xdot[4],xdot[5] */
        a = 1.5 * J2_GLO * MU_GLO * SQR(RE_GLO) / r2 / r3; /* 3/2*J2*mu*Ae^2/r^5 */
        b = 5.0 * x[2] * x[2] / r2;                    /* 5*z^2/r^2 */
        c = -MU_GLO / r3 - a * (1.0 - b);                /* -mu/r^3-a(1-b) */
        xdot[0] = x[3];
        xdot[1] = x[4];
        xdot[2] = x[5];
        xdot[3] = (c + omg2) * x[0] + 2.0 * OMGE_GLO * x[4] + acc[0];
        xdot[4] = (c + omg2) * x[1] - 2.0 * OMGE_GLO * x[3] + acc[1];
        xdot[5] = (c - 2.0 * a) * x[2] + acc[2];
    }

    /* glonass position and velocity by numerical integration --------------------*/
    static void glorbit(double t, double x[], double acc[]) {
        double k1[] = new double[6];
        double k2[] = new double[6];
        double k3[] = new double[6];
        double k4[] = new double[6];
        double w[] = new double[6];
        int i;

        deq(x, k1, acc);
        for (i = 0; i < 6; i++) w[i] = x[i] + k1[i] * t / 2.0;
        deq(w, k2, acc);
        for (i = 0; i < 6; i++) w[i] = x[i] + k2[i] * t / 2.0;
        deq(w, k3, acc);
        for (i = 0; i < 6; i++) w[i] = x[i] + k3[i] * t;
        deq(w, k4, acc);
        for (i = 0; i < 6; i++) x[i] += (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]) * t / 6.0;
    }

    public SatellitePosition computeSatPositionAndVelocities(long unixTime, double obsPseudorange, int satID, char satType, EphGlonass eph, double receiverClockError) {
//					System.out.println("### GLONASS computation");
        double X = eph.getX() * 1000;  // satellite X coordinate at ephemeris reference time
        double Y = eph.getY() * 1000;  // satellite Y coordinate at ephemeris reference time
        double Z = eph.getZ() * 1000;  // satellite Z coordinate at ephemeris reference time
        double Xv = eph.getXv() * 1000;  // satellite velocity along X at ephemeris reference time
        double Yv = eph.getYv() * 1000;  // satellite velocity along Y at ephemeris reference time
        double Zv = eph.getZv() * 1000;  // satellite velocity along Z at ephemeris reference time
        double Xa = eph.getXa() * 1000;  // acceleration due to lunar-solar gravitational perturbation along X at ephemeris reference time
        double Ya = eph.getYa() * 1000;  // acceleration due to lunar-solar gravitational perturbation along Y at ephemeris reference time
        double Za = eph.getZa() * 1000;  // acceleration due to lunar-solar gravitational perturbation along Z at ephemeris reference time
        /* NOTE:  Xa,Ya,Za are considered constant within the integration interval (i.e. toe ?}15 minutes) */

        double tn = eph.getTauN();
        float gammaN = eph.getGammaN();
        double tk = eph.gettk();
        double En = eph.getEn();
        double toc = eph.getToc();
        double toe = eph.getToe();
        int freqNum = eph.getfreq_num();

        /* integration step */
        int int_step = 60; // [s]

        /* Compute satellite clock error */
        double satelliteClockError = computeSatelliteClockError(unixTime, eph, obsPseudorange);
//				    System.out.println("satelliteClockError: " + satelliteClockError);

        /* Compute clock corrected transmission time */
        double tGPS = computeClockCorrectedTransmissionTime(unixTime, satelliteClockError, obsPseudorange);
//				    System.out.println("tGPS: " + tGPS);

        /* Time from the ephemerides reference epoch */
        Time reftime = new Time(eph.getWeek(), tGPS);
        double tk2 = checkGpsTime(tGPS - toe);
//        double tk2 = checkGpsTime(tGPS - toe + reftime.getLeapSeconds());
        System.out.println("tk2: " + tk2 + "tGPS: " + tGPS + "toe：" + toe);

//        double tt;
//        double x[] = new double[6];
//        x[0] = X;
//        x[1] = Y;
//        x[2] = Z;
//        x[3] = Xv;
//        x[4] = Yv;
//        x[5] = Zv;
//
//        double acc[] = new double[3];
//        acc[0] = Xa;
//        acc[1] = Ya;
//        acc[2] = Za;
//        for (tt = tk2 < 0.0 ? - int_step : int_step; Math.abs(tk2) > 1E-9; tk2 -= tt) {
//            if (Math.abs(tk2) < int_step) tt = tk2;
//            glorbit(tt, x , acc);
//        }

        /* number of iterations on "full" steps */
        int n = (int) Math.floor(Math.abs(tk2 / int_step));
//					System.out.println("Number of iterations: " + n);

        /* array containing integration steps (same sign as tk) */
        double[] array = new double[n];
        Arrays.fill(array, 1);
        SimpleMatrix tkArray = new SimpleMatrix(n, 1, true, array);

//					SimpleMatrix tkArray2  = tkArray.scale(2);
        tkArray = tkArray.scale(int_step);
        tkArray = tkArray.scale(tk2 / Math.abs(tk2));
//					tkArray.print();
        //double ii = tkArray * int_step * (tk2/Math.abs(tk2));

        /* check residual iteration step (i.e. remaining fraction of int_step) */
        double int_step_res = tk2 % int_step;
//				    System.out.println("int_step_res: " + int_step_res);

        double[] intStepRes = new double[]{int_step_res};
        SimpleMatrix int_stepArray = new SimpleMatrix(1, 1, false, intStepRes);
//					int_stepArray.print();

        /* adjust the total number of iterations and the array of iteration steps */
        if (int_step_res != 0) {
            tkArray = tkArray.combine(n, 0, int_stepArray);
//				        tkArray.print();
            n = n + 1;
            // tkArray = [ii; int_step_res];
        }
//				    System.out.println("n: " + n);

        // numerical integration steps (i.e. re-calculation of satellite positions from toe to tk)
        double[] pos = {X, Y, Z};
        double[] vel = {Xv, Yv, Zv};
        double[] acc = {Xa, Ya, Za};
        double[] pos1;
        double[] vel1;

        SimpleMatrix posArray = new SimpleMatrix(1, 3, true, pos);
        SimpleMatrix velArray = new SimpleMatrix(1, 3, true, vel);
        SimpleMatrix accArray = new SimpleMatrix(1, 3, true, acc);
        SimpleMatrix pos1Array;
        SimpleMatrix vel1Array;
        SimpleMatrix pos2Array;
        SimpleMatrix vel2Array;
        SimpleMatrix pos3Array;
        SimpleMatrix vel3Array;
        SimpleMatrix pos4Array;
        SimpleMatrix vel4Array;
        SimpleMatrix pos1dotArray;
        SimpleMatrix vel1dotArray;
        SimpleMatrix pos2dotArray;
        SimpleMatrix vel2dotArray;
        SimpleMatrix pos3dotArray;
        SimpleMatrix vel3dotArray;
        SimpleMatrix pos4dotArray;
        SimpleMatrix vel4dotArray;
        SimpleMatrix subPosArray;
        SimpleMatrix subVelArray;

        for (int i = 0; i < n; i++) {

            /* Runge-Kutta numerical integration algorithm */
            // step 1
            pos1Array = posArray;
            //pos1 = pos;
            vel1Array = velArray;
            //vel1 = vel;

            // differential position
            pos1dotArray = velArray;
            //double[] pos1_dot = vel;
            vel1dotArray = satellite_motion_diff_eq(pos1Array, vel1Array, accArray, Constants.ELL_A_GLO, Constants.GM_GLO, Constants.J2_GLO, Constants.OMEGAE_DOT_GLO);
            //double[] vel1_dot = satellite_motion_diff_eq(pos1, vel1, acc, Constants.ELL_A_GLO, Constants.GM_GLO, Constants.J2_GLO, Constants.OMEGAE_DOT_GLO);
//							vel1dotArray.print();

            // step 2
            pos2Array = pos1dotArray.scale(tkArray.get(i)).divide(2);
            pos2Array = posArray.plus(pos2Array);
            //double[] pos2 = pos + pos1_dot*ii(i)/2;
//							System.out.println("## pos2Array: " ); pos2Array.print();

            vel2Array = vel1dotArray.scale(tkArray.get(i)).divide(2);
            vel2Array = velArray.plus(vel2Array);
            //double[] vel2 = vel + vel1_dot * tkArray.get(i)/2;
//							System.out.println("## vel2Array: " ); vel2Array.print();

            pos2dotArray = vel2Array;
            //double[] pos2_dot = vel2;
            vel2dotArray = satellite_motion_diff_eq(pos2Array, vel2Array, accArray, Constants.ELL_A_GLO, Constants.GM_GLO, Constants.J2_GLO, Constants.OMEGAE_DOT_GLO);
            //double[] vel2_dot = satellite_motion_diff_eq(pos2, vel2, acc, Constants.ELL_A_GLO, Constants.GM_GLO, Constants.J2_GLO, Constants.OMEGAE_DOT_GLO);
//							System.out.println("## vel2dotArray: " ); vel2dotArray.print();

            // step 3
            pos3Array = pos2dotArray.scale(tkArray.get(i)).divide(2);
            pos3Array = posArray.plus(pos3Array);
//							double[] pos3 = pos + pos2_dot * tkArray.get(i)/2;
//							System.out.println("## pos3Array: " ); pos3Array.print();

            vel3Array = vel2dotArray.scale(tkArray.get(i)).divide(2);
            vel3Array = velArray.plus(vel3Array);
//					        double[] vel3 = vel + vel2_dot * tkArray.get(i)/2;
//							System.out.println("## vel3Array: " ); vel3Array.print();

            pos3dotArray = vel3Array;
            //double[] pos3_dot = vel3;
            vel3dotArray = satellite_motion_diff_eq(pos3Array, vel3Array, accArray, Constants.ELL_A_GLO, Constants.GM_GLO, Constants.J2_GLO, Constants.OMEGAE_DOT_GLO);
            //double[] vel3_dot = satellite_motion_diff_eq(pos3, vel3, acc, Constants.ELL_A_GLO, Constants.GM_GLO, Constants.J2_GLO, Constants.OMEGAE_DOT_GLO);
//							System.out.println("## vel3dotArray: " ); vel3dotArray.print();

            // step 4
            pos4Array = pos3dotArray.scale(tkArray.get(i));
            pos4Array = posArray.plus(pos4Array);
            //double[] pos4 = pos + pos3_dot * tkArray.get(i);
//							System.out.println("## pos4Array: " ); pos4Array.print();

            vel4Array = vel3dotArray.scale(tkArray.get(i));
            vel4Array = velArray.plus(vel4Array);
            //double[] vel4 = vel + vel3_dot * tkArray.get(i);
//							System.out.println("## vel4Array: " ); vel4Array.print();

            pos4dotArray = vel4Array;
            //double[] pos4_dot = vel4;
            vel4dotArray = satellite_motion_diff_eq(pos4Array, vel4Array, accArray, Constants.ELL_A_GLO, Constants.GM_GLO, Constants.J2_GLO, Constants.OMEGAE_DOT_GLO);
            //double[] vel4_dot = satellite_motion_diff_eq(pos4, vel4, acc, Constants.ELL_A_GLO, Constants.GM_GLO, Constants.J2_GLO, Constants.OMEGAE_DOT_GLO);
//							System.out.println("## vel4dotArray: " ); vel4dotArray.print();

            // final position and velocity
            subPosArray = pos1dotArray.plus(pos2dotArray.scale(2)).plus(pos3dotArray.scale(2)).plus(pos4dotArray);
            subPosArray = subPosArray.scale(tkArray.get(i)).divide(6);
            posArray = posArray.plus(subPosArray);
            //pos = pos + (pos1_dot + 2*pos2_dot + 2*pos3_dot + pos4_dot)*ii(s)/6;
//							System.out.println("## posArray: " ); posArray.print();

            subVelArray = vel1dotArray.plus(vel2dotArray.scale(2)).plus(vel3dotArray.scale(2)).plus(vel4dotArray);
            subVelArray = subVelArray.scale(tkArray.get(i)).divide(6);
            velArray = velArray.plus(subVelArray);
            //vel = vel + (vel1_dot + 2*vel2_dot + 2*vel3_dot + vel4_dot)*ii(s)/6;
//							System.out.println("## velArray: " ); velArray.print();
//							System.out.println(" " );


        }

        /* transformation from PZ-90.02 to WGS-84 (G1150) */
        double x1 = posArray.get(0) - 0.36;
        double y1 = posArray.get(1) + 0.08;
        double z1 = posArray.get(2) + 0.18;

        /* satellite velocity */
        double Xv1 = velArray.get(0);
        double Yv1 = velArray.get(1);
        double Zv1 = velArray.get(2);

        /* Fill in the satellite position matrix */
        SatellitePosition sp = new SatellitePosition(unixTime, satID, satType, x1, y1, z1);
        sp.setSatelliteClockError(satelliteClockError);
//
//					/* Apply the correction due to the Earth rotation during signal travel time */
        SimpleMatrix R = computeEarthRotationCorrection(unixTime, receiverClockError, tGPS);
        sp.setSMMultXYZ(R);

        return sp;
//					return null ;
    }

    private SimpleMatrix satellite_motion_diff_eq(SimpleMatrix pos1Array,
                                                  SimpleMatrix vel1Array, SimpleMatrix accArray, long ellAGlo,
                                                  double gmGlo, double j2Glo, double omegaeDotGlo) {
        // TODO Auto-generated method stub

        /* renaming variables for better readability position */
        double X = pos1Array.get(0);
        double Y = pos1Array.get(1);
        double Z = pos1Array.get(2);

//		System.out.println("X: " + X);
//		System.out.println("Y: " + Y);
//		System.out.println("Z: " + Z);

        /* velocity */
        double Xv = vel1Array.get(0);
        double Yv = vel1Array.get(1);

//		System.out.println("Xv: " + Xv);
//		System.out.println("Yv: " + Yv);

        /* acceleration (i.e. perturbation) */
        double Xa = accArray.get(0);
        double Ya = accArray.get(1);
        double Za = accArray.get(2);

//		System.out.println("Xa: " + Xa);
//		System.out.println("Ya: " + Ya);
//		System.out.println("Za: " + Za);

        /* parameters */
        double r = Math.sqrt(Math.pow(X, 2) + Math.pow(Y, 2) + Math.pow(Z, 2));
        double g = -gmGlo / Math.pow(r, 3);
        double h = j2Glo * 1.5 * Math.pow((ellAGlo / r), 2);
        double k = 5 * Math.pow(Z, 2) / Math.pow(r, 2);

//		System.out.println("r: " + r);
//		System.out.println("g: " + g);
//		System.out.println("h: " + h);
//		System.out.println("k: " + k);

        /* differential velocity */
        double[] vel_dot = new double[3];
        vel_dot[0] = g * X * (1 - h * (k - 1)) + Xa + Math.pow(omegaeDotGlo, 2) * X + 2 * omegaeDotGlo * Yv;
//		System.out.println("vel1: " + vel_dot[0]);

        vel_dot[1] = g * Y * (1 - h * (k - 1)) + Ya + Math.pow(omegaeDotGlo, 2) * Y - 2 * omegaeDotGlo * Xv;
//		System.out.println("vel2: " + vel_dot[1]);

        vel_dot[2] = g * Z * (1 - h * (k - 3)) + Za;
//		System.out.println("vel3: " + vel_dot[2]);

        SimpleMatrix velDotArray = new SimpleMatrix(1, 3, true, vel_dot);
//		velDotArray.print();

        return velDotArray;
    }


    /**
     * @param time (Uncorrected GPS time)
     * @return GPS time accounting for beginning or end of week crossover
     */
    protected double checkGpsTime(double time) {

        // Account for beginning or end of week crossover
        if (time > Constants.SEC_IN_HALF_WEEK) {
            time = time - 2 * Constants.SEC_IN_HALF_WEEK;
        } else if (time < -Constants.SEC_IN_HALF_WEEK) {
            time = time + 2 * Constants.SEC_IN_HALF_WEEK;
        }
        return time;
    }

    /**
     * @param traveltime
     */
    protected SimpleMatrix computeEarthRotationCorrection(long unixTime, double receiverClockError, double transmissionTime) {

        // Computation of signal travel time
        // SimpleMatrix diff = satellitePosition.minusXYZ(approxPos);//this.coord.minusXYZ(approxPos);
        // double rho2 = Math.pow(diff.get(0), 2) + Math.pow(diff.get(1), 2)
        // 		+ Math.pow(diff.get(2), 2);
        // double traveltime = Math.sqrt(rho2) / Constants.SPEED_OF_LIGHT;
        double receptionTime = (new Time(unixTime)).getGpsTime();
        double traveltime = receptionTime + receiverClockError - transmissionTime;

        // Compute rotation angle
        double omegatau = Constants.EARTH_ANGULAR_VELOCITY * traveltime;

        // Rotation matrix
        double[][] data = new double[3][3];
        data[0][0] = Math.cos(omegatau);
        data[0][1] = Math.sin(omegatau);
        data[0][2] = 0;
        data[1][0] = -Math.sin(omegatau);
        data[1][1] = Math.cos(omegatau);
        data[1][2] = 0;
        data[2][0] = 0;
        data[2][1] = 0;
        data[2][2] = 1;
        SimpleMatrix R = new SimpleMatrix(data);

        return R;
        // Apply rotation
        //this.coord.ecef = R.mult(this.coord.ecef);
        //this.coord.setSMMultXYZ(R);// = R.mult(this.coord.ecef);
        //satellitePosition.setSMMultXYZ(R);// = R.mult(this.coord.ecef);

    }

    /**
     * @param eph
     * @return Clock-corrected GPS transmission time
     */
    protected double computeClockCorrectedTransmissionTime(long unixTime, double satelliteClockError, double obsPseudorange) {

        double gpsTime = (new Time(unixTime)).getGpsTime();

        // Remove signal travel time from observation time
        double tRaw = (gpsTime - obsPseudorange /*this.range*/ / Constants.SPEED_OF_LIGHT);

        return tRaw - satelliteClockError;
    }

    /**
     * @param eph
     * @return Satellite clock error
     */
    protected double computeSatelliteClockError(long unixTime, EphGlonass eph, double obsPseudorange) {
        double gpsTime = (new Time(unixTime)).getGpsTime();

        // Remove signal travel time from observation time
        double tRaw = (gpsTime - obsPseudorange /*this.range*/ / Constants.SPEED_OF_LIGHT);

        // Clock error computation
        double dt = checkGpsTime(tRaw - eph.getToe());

        double timeCorrection = eph.getTauN() + eph.getGammaN() * dt;

        return timeCorrection;
    }

}
